// Copyright 2015 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <iostream>

// include ROS 1
#ifdef __clang__
# pragma clang diagnostic push
# pragma clang diagnostic ignored "-Wunused-parameter"
#endif
#include "ros/ros.h"
#include "sensor_msgs/PointCloud2.h"
#include "sensor_msgs/PointField.h"
#ifdef __clang__
# pragma clang diagnostic pop
#endif

// include ROS 2
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"


class PointCloudTrans : public rclcpp::Node
{
  public:
    ros::NodeHandle n;
    std::string m_vehicle_name;
    ros::Publisher pub;

    PointCloudTrans()
    : Node("pts_subscriber")
    {
      this->declare_parameter("vehicle_name");
        m_vehicle_name = this->get_parameter("vehicle_name").as_string();
      
      std::string topic_name = "/" + m_vehicle_name + "/slot7/points";

      pub = n.advertise<sensor_msgs::PointCloud2>(topic_name, 1);
      

      subscription_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
      topic_name, 1, std::bind(&PointCloudTrans::pointCloudCallback, this, std::placeholders::_1));
    }

  private:
    void pointCloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr ros2_msg)
    {
      // rclcpp::RCLCPP_INFO("I heard image");
      sensor_msgs::PointCloud2 ros1_msg;
      ros1_msg.header.frame_id = ros2_msg->header.frame_id;
      ros1_msg.header.stamp.sec = ros2_msg->header.stamp.sec;
      ros1_msg.header.stamp.nsec = ros2_msg->header.stamp.nanosec;
      ros1_msg.height = ros2_msg->height;
      ros1_msg.width = ros2_msg->width;
      
      for(int i = 0; i < ros2_msg->fields.size(); i++)
      {
        sensor_msgs::PointField pointfild;
        pointfild.name = ros2_msg->fields[i].name;
        pointfild.offset = ros2_msg->fields[i].offset;
        pointfild.datatype = ros2_msg->fields[i].datatype;
        pointfild.count = ros2_msg->fields[i].count;
        ros1_msg.fields.push_back(pointfild);
      }
      ros1_msg.is_bigendian = ros2_msg->is_bigendian;
      ros1_msg.point_step = ros2_msg->point_step;
      ros1_msg.row_step = ros2_msg->row_step;
      ros1_msg.data.assign(ros2_msg->data.begin(), ros2_msg->data.end());

      // ros1_msg.data = ros2_msg->data;
      // std::cout << "I heard points: " << i<< std::endl;
      // i++;
      // rclcpp::RCLCPP_INFO("publish this image");
      pub.publish(ros1_msg);
    }
    rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr subscription_;
};

int main(int argc, char ** argv)
{
  // char * r_argv = i.c_str();
  int r_argc = 1;
  // std::cout<<topic_name<<std::endl;
  ros::init(r_argc, argv,"pointcloud_node",ros::init_options::AnonymousName);
  
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<PointCloudTrans>());
  rclcpp::shutdown();
  return 0;
}